#include "dualchessboard.h"
#include "markerchessboard.hpp"

DualChessboard::DualChessboard(const CvSize &size)
{
	CV_Assert(size.width >=2 && size.height >= 2);

	const float unitLength = 16.5; //arbitrary postion number
	m_patternSize = size;
	m_markerCenters.push_back(Point2f(-unitLength, -unitLength));
	m_markerCenters.push_back(Point2f(unitLength*size.width, -unitLength));
	m_markerCenters.push_back(Point2f(unitLength*size.width, unitLength*size.height));
	m_markerCenters.push_back(Point2f(-unitLength, unitLength*size.height));

	m_boardCorners.resize(size.height*size.width);
	for (int y = 0; y<size.height; ++y)
	{
		for (int x = 0; x<size.width; ++x)
		{
			float f_x = x * unitLength;
			float f_y = y * unitLength;
			int idx = y * size.width + x;
			m_boardCorners[idx] = Point2f(f_x, f_y);
		}
	}
}
DualChessboard::~DualChessboard()
{

}

bool DualChessboard::findCorners(const Mat &image, bool isTracking, vector<Point2f> &firstCorners, 
					 vector<Point2f> &secondCorners, int distortionThreshold)
{
	//check parameters
	CV_Assert(!image.empty());
	CV_Assert(image.depth() == CV_8U);
	
	Mat grayImage;
	if (image.channels() == 1)
	{
		grayImage = image;
	}
	else 
	{
		cv::cvtColor(image, grayImage, CV_BGR2GRAY);
	}

	int h = image.rows;
	int w = image.cols;

//	Mat testImage = image.clone();

	//get marker parameters
	MarkerParameters mParameters;		
	CvMarkerChessboard::GetThreshold(cvSize(w, h), m_patternSize, mParameters.maxPixels, mParameters.minPixels);

	//tracking marker position in four sub area of the image
	/*
	curMarkers.clear();
	if (isTracking && preMarkers.size() == 4)
	{
		for (unsigned int i = 0; i<4; ++i)
		{
			CvRect r = preMarkers[i].subArea;
			vector<CvxMarker> subMarker;
			bool rst = CvxMarker::detect(grayImage(r), 1, subMarker,  mParameters);			
			if (rst)
			{
				subMarker[0].addOffset(r.x, r.y);
				curMarkers.push_back(subMarker[0]);
			}
			else
			{
				curMarkers.clear();
				break;
			}
		}		
	}
	else
	*/
	{
		//detecting in whole image
		bool ret = CvxMarker::detect(grayImage, 8, m_firstCurMarkers, mParameters);
		if (ret)
		{		
			m_secondCurMarkers.clear();
			for(int i = 0; i<4; i++)
			{
				m_secondCurMarkers.push_back(m_firstCurMarkers[4+i]);
			}
			m_firstCurMarkers.resize(4);
		}

#define TEST_DualChessboard 0
#if TEST_DualChessboard
		//test
		for(int i =0; i<4; i++)
		{
			cv::circle(testImage, m_firstCurMarkers[i].center, 2, Scalar(255, 0, 0), 1);
			cv::circle(testImage, m_secondCurMarkers[i].center, 2, Scalar(0, 0, 255), 1); 

		}
		cv::imshow("center position", testImage);
#endif

	}	
	
	if (m_firstCurMarkers.size() == 4 && m_secondCurMarkers.size() == 4)
	{
		for (unsigned int i = 0; i<4; ++i)
		{
			m_firstCurMarkers[i].getCenterPoints();
			m_firstCurMarkers[i].getSubArea(10, w, h);

			m_secondCurMarkers[i].getCenterPoints();
			m_secondCurMarkers[i].getSubArea(10, w, h);
		}
	}
	else
	{
		return false;
	}

	//get first chessboard corners
	bool isFound = calcCornersFromMakerCenters(m_firstCurMarkers, firstCorners, w, h, distortionThreshold, grayImage);
	if(!isFound)
	{
		return false;		
	}

	//get second chessboard corners
	isFound = calcCornersFromMakerCenters(m_secondCurMarkers, secondCorners, w, h, distortionThreshold, grayImage);
	if(!isFound)
	{
		firstCorners.clear();
		return false;		
	}

	m_firstPreMarkers = m_firstCurMarkers;
	m_secondPreMarkers = m_secondCurMarkers;
	return true;	
}

bool DualChessboard::calcCornersFromMakerCenters(const vector<CvxMarker> & markers, vector<Point2f> &corners, int w, int h, int distortionThreshold, const Mat &grayImage)
{	
	//get H
	CV_Assert(m_markerCenters.size() == 4);
	CV_Assert(markers.size() == 4);

	vector<Point2f> markerCentersInImage;
	for (unsigned int i = 0; i<markers.size(); ++i)
	{
		markerCentersInImage.push_back(markers[i].center);
	}
	Mat homo = CvMarkerChessboard::findHomography4Pts(m_markerCenters, markerCentersInImage);

	if(homo.empty())
	{
		return false;
	}

	//get initial chessboard corner position
	corners.resize(m_patternSize.width * m_patternSize.height);
	double *pData = (double*)(homo.data);
	for (unsigned int i = 0; i<m_boardCorners.size(); ++i)
	{
		Point2f p1 = m_boardCorners[i];
		Point2f p2;
		float scale = 1.0;
		p2.x  = (float)(pData[0] * p1.x  + pData[1] * p1.y + pData[2]);
		p2.y  = (float)(pData[3] * p1.x  + pData[4] * p1.y + pData[5]);
		scale = (float)(pData[6] * p1.x  + pData[7] * p1.y + pData[8]);
		p2.x /= scale;
		p2.y /= scale;
		CV_Assert(p2.x >=0 && p2.x < w);
		CV_Assert(p2.y >=0 && p2.y < h);
		corners[i] = p2;
	}

	//get refined corner position
	vector<Point2f> unRef = corners;
	int winLength = 11;
	if (w < 800)
	{
		winLength = 5;
	}	
	cv::cornerSubPix(grayImage, corners, Size(winLength, winLength), Size(-1, -1), 
						TermCriteria( CV_TERMCRIT_EPS+CV_TERMCRIT_ITER, 30, 0.1 ));


	//check final points with maxiDistortion
	for (unsigned int i = 0; i<corners.size(); ++i)
	{
		if (fabs(unRef[i].x - corners[i].x) > distortionThreshold 
			|| fabs(unRef[i].y - corners[i].y) > distortionThreshold) 
		{			
			corners.clear();
			return false;
		}
	}	
	return true;
}